import java.util.*;

// 路径规划策略接口 (策略模式)
// 路径点类
public class PathPoint {
    private double x;
    private double y;
    private String description;

    public PathPoint(double x, double y, String description) {
        this.x = x;
        this.y = y;
        this.description = description;
    }

    public double getX() { return x; }
    public double getY() { return y; }
    public String getDescription() { return description; }

    public double distanceTo(PathPoint other) {
        double dx = x - other.x;
        double dy = y - other.y;
        return Math.sqrt(dx*dx + dy*dy);
    }

    @Override
    public String toString() {
        return String.format("(%.1f, %.1f): %s", x, y, description);
    }
}

// 路径类
class Path {
    private List<PathPoint> points = new ArrayList<>();
    private double totalLength;

    public void addPoint(PathPoint point) {
        if (!points.isEmpty()) {
            totalLength += points.get(points.size()-1).distanceTo(point);
        }
        points.add(point);
    }

    public List<PathPoint> getPoints() { return points; }
    public double getTotalLength() { return totalLength; }

    public void display() {
        System.out.println("=== 路径详情 ===");
        System.out.printf("总长度: %.1f 米\n", totalLength);
        System.out.println("路径点:");
        for (int i = 0; i < points.size(); i++) {
            System.out.printf("%d. %s\n", i+1, points.get(i));
        }
    }
}

// 路径规划策略接口 (策略模式)
interface PathPlanningStrategy {
    Path calculatePath(String fieldId, String machineId, Map<String, Object> context);
    String getStrategyName();
}

// 基础路径规划策略
class BasicPathStrategy implements PathPlanningStrategy {
    @Override
    public Path calculatePath(String fieldId, String machineId, Map<String, Object> context) {
        Path path = new Path();
        path.addPoint(new PathPoint(0, 0, "起点"));
        path.addPoint(new PathPoint(50, 0, "作业区1"));
        path.addPoint(new PathPoint(50, 50, "作业区2"));
        path.addPoint(new PathPoint(0, 50, "作业区3"));
        path.addPoint(new PathPoint(0, 0, "终点"));
        return path;
    }

    @Override
    public String getStrategyName() {
        return "基础直线路径";
    }
}

// 高效覆盖路径策略 (蛇形路径)
class EfficientCoverageStrategy implements PathPlanningStrategy {
    @Override
    public Path calculatePath(String fieldId, String machineId, Map<String, Object> context) {
        Path path = new Path();
        path.addPoint(new PathPoint(0, 0, "起点"));

        int rows = 5;
        double width = 50.0;
        double height = 50.0;
        double rowSpacing = height / rows;

        for (int i = 0; i < rows; i++) {
            if (i % 2 == 0) {
                path.addPoint(new PathPoint(width, i * rowSpacing, "作业行" + (i+1) + "右端"));
                path.addPoint(new PathPoint(width, (i+1) * rowSpacing, "转向点"));
            } else {
                path.addPoint(new PathPoint(0, (i+1) * rowSpacing, "作业行" + (i+1) + "左端"));
                if (i < rows - 1) {
                    path.addPoint(new PathPoint(0, (i+1) * rowSpacing, "转向点"));
                }
            }
        }

        path.addPoint(new PathPoint(0, height, "终点"));
        return path;
    }

    @Override
    public String getStrategyName() {
        return "高效覆盖路径";
    }
}

// 避障路径策略
class ObstacleAvoidanceStrategy implements PathPlanningStrategy {
    @Override
    public Path calculatePath(String fieldId, String machineId, Map<String, Object> context) {
        Path path = new Path();
        path.addPoint(new PathPoint(0, 0, "起点"));

        // 模拟障碍物位置
        double obstacleX = 30.0;
        double obstacleY = 30.0;
        double obstacleRadius = 10.0;

        // 绕过障碍物的路径
        path.addPoint(new PathPoint(20, 0, "接近障碍物"));
        path.addPoint(new PathPoint(20, 20, "绕过障碍物1"));
        path.addPoint(new PathPoint(40, 20, "绕过障碍物2"));
        path.addPoint(new PathPoint(40, 40, "绕过障碍物3"));
        path.addPoint(new PathPoint(20, 40, "绕过障碍物4"));
        path.addPoint(new PathPoint(20, 50, "返回主路径"));
        path.addPoint(new PathPoint(0, 50, "终点"));

        // 将障碍物信息存入上下文
        if (context != null) {
            context.put("obstacleX", obstacleX);
            context.put("obstacleY", obstacleY);
            context.put("obstacleRadius", obstacleRadius);
        }

        return path;
    }

    @Override
    public String getStrategyName() {
        return "避障路径";
    }
}

// 协同路径规划策略
class CollaborativePathStrategy implements PathPlanningStrategy {
    @Override
    public Path calculatePath(String fieldId, String machineId, Map<String, Object> context) {
        Path path = new Path();

        // 根据农机ID分配不同的路径区域
        if ("machine1".equals(machineId)) {
            path.addPoint(new PathPoint(0, 0, "起点-农机1"));
            path.addPoint(new PathPoint(25, 0, "作业区1-农机1"));
            path.addPoint(new PathPoint(25, 50, "作业区2-农机1"));
            path.addPoint(new PathPoint(0, 50, "终点-农机1"));
        } else if ("machine2".equals(machineId)) {
            path.addPoint(new PathPoint(25, 0, "起点-农机2"));
            path.addPoint(new PathPoint(50, 0, "作业区1-农机2"));
            path.addPoint(new PathPoint(50, 50, "作业区2-农机2"));
            path.addPoint(new PathPoint(25, 50, "终点-农机2"));
        } else {
            // 默认路径
            path.addPoint(new PathPoint(0, 0, "起点"));
            path.addPoint(new PathPoint(50, 0, "作业区1"));
            path.addPoint(new PathPoint(50, 50, "作业区2"));
            path.addPoint(new PathPoint(0, 50, "终点"));
        }

        return path;
    }

    @Override
    public String getStrategyName() {
        return "协同作业路径";
    }
}// 路径规划上下文
class PathPlannerContext {
    private PathPlanningStrategy strategy;
    private Map<String, Object> contextData = new HashMap<>();

    public void setStrategy(PathPlanningStrategy strategy) {
        this.strategy = strategy;
    }

    public Path executeStrategy(String fieldId, String machineId) {
        if (strategy == null) {
            throw new IllegalStateException("策略未设置");
        }
        return strategy.calculatePath(fieldId, machineId, contextData);
    }

    public void addContextData(String key, Object value) {
        contextData.put(key, value);
    }

    public Object getContextData(String key) {
        return contextData.get(key);
    }
}

// 路径优化器
class PathOptimizer {
    // 路径平滑处理
    public static Path smoothPath(Path originalPath) {
        Path smoothed = new Path();
        List<PathPoint> points = originalPath.getPoints();

        if (points.isEmpty()) return smoothed;

        smoothed.addPoint(points.get(0));

        for (int i = 1; i < points.size() - 1; i++) {
            // 简单平滑 - 跳过一些中间点
            if (i % 2 == 0) {
                smoothed.addPoint(points.get(i));
            }
        }

        if (points.size() > 1) {
            smoothed.addPoint(points.get(points.size() - 1));
        }

        return smoothed;
    }

    // 路径重规划（遇到障碍物时）
    public static Path replanPathForObstacle(Path originalPath, double obstacleX, double obstacleY, double radius) {
        Path newPath = new Path();
        List<PathPoint> points = originalPath.getPoints();

        if (points.isEmpty()) return newPath;

        newPath.addPoint(points.get(0));

        for (int i = 1; i < points.size(); i++) {
            PathPoint prev = points.get(i-1);
            PathPoint current = points.get(i);

            // 检查路径段是否穿过障碍物
            if (isSegmentCrossingObstacle(prev, current, obstacleX, obstacleY, radius)) {
                // 添加避障点
                List<PathPoint> detour = createDetourPoints(prev, current, obstacleX, obstacleY, radius);
                newPath.getPoints().addAll(detour);
            } else {
                newPath.addPoint(current);
            }
        }

        return newPath;
    }

    private static boolean isSegmentCrossingObstacle(PathPoint p1, PathPoint p2,
                                                     double ox, double oy, double r) {
        // 简化的障碍物碰撞检测
        double minX = Math.min(p1.getX(), p2.getX());
        double maxX = Math.max(p1.getX(), p2.getX());
        double minY = Math.min(p1.getY(), p2.getY());
        double maxY = Math.max(p1.getY(), p2.getY());

        // 检查障碍物是否在路径段的边界框内
        if (ox >= minX - r && ox <= maxX + r &&
                oy >= minY - r && oy <= maxY + r) {
            return true;
        }
        return false;
    }

    private static List<PathPoint> createDetourPoints(PathPoint p1, PathPoint p2,
                                                      double ox, double oy, double r) {
        List<PathPoint> detour = new ArrayList<>();
        double safeDistance = r + 5.0; // 安全距离

        // 简单的绕过障碍物的路径 (实际应用中可以使用更复杂的算法)
        if (Math.abs(p2.getX() - p1.getX()) > Math.abs(p2.getY() - p1.getY())) {
            // 主要是水平移动
            double detourY = oy + (p1.getY() < oy ? -safeDistance : safeDistance);
            detour.add(new PathPoint(p1.getX(), detourY, "避障点1"));
            detour.add(new PathPoint(p2.getX(), detourY, "避障点2"));
        } else {
            // 主要是垂直移动
            double detourX = ox + (p1.getX() < ox ? -safeDistance : safeDistance);
            detour.add(new PathPoint(detourX, p1.getY(), "避障点1"));
            detour.add(new PathPoint(detourX, p2.getY(), "避障点2"));
        }

        return detour;
    }

    // 协同路径检查（避免农机碰撞）
    public static void checkForCollisions(Path path1, Path path2) {
        // 简化的碰撞检测（实际应用中需要更精确的时间空间检查）
        List<PathPoint> points1 = path1.getPoints();
        List<PathPoint> points2 = path2.getPoints();

        int minSize = Math.min(points1.size(), points2.size());
        for (int i = 0; i < minSize; i++) {
            double distance = points1.get(i).distanceTo(points2.get(i));
            if (distance < 5.0) { // 安全距离
                System.out.printf("警告: 农机可能在点 %d (%.1f,%.1f) 和 (%.1f,%.1f) 处发生碰撞 (距离: %.1f米)%n",
                        i+1,
                        points1.get(i).getX(), points1.get(i).getY(),
                        points2.get(i).getX(), points2.get(i).getY(),
                        distance);
            }
        }
    }
}class PathPlanner {
    private PathPlannerContext context = new PathPlannerContext();
    private Map<String, Path> machinePaths = new HashMap<>();

    public void showMenu() {
        Scanner scanner = new Scanner(System.in);
        while (true) {
<<<<<<< HEAD
            System.out.println("\n=== 智能路径规划 ===");
            System.out.println("1. 基础路径规划");
            System.out.println("2. 高效覆盖路径");
            System.out.println("3. 避障路径规划");
=======


>>>>>>> b218c59c5b5bab9a1718d1c599ba18ad4130f453
            System.out.println("4. 协同路径规划");
            System.out.println("5. 路径优化");
            System.out.println("6. 显示农机路径");
            System.out.println("0. 返回上级菜单");

            int choice = scanner.nextInt();
            scanner.nextLine(); // 消耗换行符

            switch (choice) {
                case 1:
                    planBasicPath(scanner);
                    break;
                case 2:
                    planEfficientPath(scanner);
                    break;
                case 3:
                    planObstacleAvoidancePath(scanner);
                    break;
                case 4:
                    planCollaborativePath(scanner);
                    break;
                case 5:
                    optimizePath(scanner);
                    break;
                case 6:
                    displayMachinePaths(scanner);
                    break;
                case 0:
                    return;
                default:
                    System.out.println("无效选择，请重新输入！");
            }
        }
    }

    private void planBasicPath(Scanner scanner) {
        System.out.println("输入农田ID:");
        String fieldId = scanner.nextLine();

        System.out.println("输入农机ID:");
        String machineId = scanner.nextLine();

        context.setStrategy(new BasicPathStrategy());
        Path path = context.executeStrategy(fieldId, machineId);
        machinePaths.put(machineId, path);

        System.out.println("\n生成的基础路径:");
        path.display();
    }

    private void planEfficientPath(Scanner scanner) {
        System.out.println("输入农田ID:");
        String fieldId = scanner.nextLine();

        System.out.println("输入农机ID:");
        String machineId = scanner.nextLine();

        context.setStrategy(new EfficientCoverageStrategy());
        Path path = context.executeStrategy(fieldId, machineId);
        machinePaths.put(machineId, path);

        System.out.println("\n生成的高效覆盖路径:");
        path.display();
    }

    private void planObstacleAvoidancePath(Scanner scanner) {
        System.out.println("输入农田ID:");
        String fieldId = scanner.nextLine();

        System.out.println("输入农机ID:");
        String machineId = scanner.nextLine();

        // 添加障碍物信息到上下文
        System.out.println("输入障碍物X坐标:");
        double ox = scanner.nextDouble();
        System.out.println("输入障碍物Y坐标:");
        double oy = scanner.nextDouble();
        System.out.println("输入障碍物半径:");
        double radius = scanner.nextDouble();

        context.addContextData("obstacleX", ox);
        context.addContextData("obstacleY", oy);
        context.addContextData("obstacleRadius", radius);

        context.setStrategy(new ObstacleAvoidanceStrategy());
        Path path = context.executeStrategy(fieldId, machineId);
        machinePaths.put(machineId, path);

        System.out.println("\n生成的避障路径:");
        path.display();
    }

    private void planCollaborativePath(Scanner scanner) {
        System.out.println("输入农田ID:");
        String fieldId = scanner.nextLine();

        System.out.println("输入农机1 ID:");
        String machineId1 = scanner.nextLine();
        System.out.println("输入农机2 ID:");
        String machineId2 = scanner.nextLine();

        // 为两台农机规划协同路径
        context.setStrategy(new CollaborativePathStrategy());

        Path path1 = context.executeStrategy(fieldId, machineId1);
        Path path2 = context.executeStrategy(fieldId, machineId2);

        machinePaths.put(machineId1, path1);
        machinePaths.put(machineId2, path2);

        System.out.println("\n生成的协同路径:");
        System.out.println("=== 农机 " + machineId1 + " 路径 ===");
        path1.display();

        System.out.println("\n=== 农机 " + machineId2 + " 路径 ===");
        path2.display();

        // 检查潜在碰撞
        System.out.println("\n协同路径碰撞检查:");
        PathOptimizer.checkForCollisions(path1, path2);
    }

    private void optimizePath(Scanner scanner) {
        System.out.println("输入农机ID:");
        String machineId = scanner.nextLine();

        Path originalPath = machinePaths.get(machineId);
        if (originalPath == null) {
            System.out.println("错误: 未找到该农机的路径！");
            return;
        }

        System.out.println("选择优化方式: 1.路径平滑 2.避障重规划");
        int option = scanner.nextInt();

        Path optimizedPath;
        switch (option) {
            case 1:
                optimizedPath = PathOptimizer.smoothPath(originalPath);
                System.out.println("\n平滑后的路径:");
                break;
            case 2:
                System.out.println("输入障碍物X坐标:");
                double ox = scanner.nextDouble();
                System.out.println("输入障碍物Y坐标:");
                double oy = scanner.nextDouble();
                System.out.println("输入障碍物半径:");
                double radius = scanner.nextDouble();

                optimizedPath = PathOptimizer.replanPathForObstacle(originalPath, ox, oy, radius);
                System.out.println("\n重规划后的避障路径:");
                break;
            default:
                System.out.println("无效选项！");
                return;
        }

        optimizedPath.display();
        machinePaths.put(machineId, optimizedPath);
    }

    private void displayMachinePaths(Scanner scanner) {
        if (machinePaths.isEmpty()) {
            System.out.println("当前没有规划的路径。");
            return;
        }

        System.out.println("\n=== 所有农机路径 ===");
        for (Map.Entry<String, Path> entry : machinePaths.entrySet()) {
            System.out.println("农机 " + entry.getKey() + " 路径:");
            entry.getValue().display();
            System.out.println();
        }
    }
}